import RPi.GPIO as GPIO
import time
from .motor import *
from .led import *

class Ultrasonic:
    
    def __init__(self):
        self.TRIGGER = 31  # pin6
        self.ECHO = 29   # pin5
        # ultrasonic
        GPIO.setup(self.TRIGGER, GPIO.OUT)
        GPIO.setup(self.ECHO, GPIO.IN)
        self.distance = self.get_distance()

    def get_distance(self):
        # set Trigger to HIGH
        GPIO.output(self.TRIGGER, True)

        # set Trigger after 0.01ms to LOW
        time.sleep(0.00001)
        GPIO.output(self.TRIGGER, False)

        StartTime = time.time()
        StopTime = time.time()

        # save StartTime
        while GPIO.input(self.ECHO) == 0:
            StartTime = time.time()

        # save time of arrival
        while GPIO.input(self.ECHO) == 1:
            StopTime = time.time()

        # time difference between start and arrival
        TimeElapsed = StopTime - StartTime
        # multiply with the sonic speed (34300 cm/s)
        # and divide by 2, because there and back
        return (TimeElapsed * 34300) / 2


    def worker(self,motor):
        """ul thread worker function"""
        while True:
            time.sleep(0.1)
            self.distance=self.get_distance()
            # print ("Measured Distance = %.1f cm" % dist) 
            if (motor.MOTOR_STATE==Motor.GO or motor.MOTOR_STATE==Motor.LEFT or motor.MOTOR_STATE==Motor.RIGHT):
                if (self.distance < 30):
                    led_stop()
                    motor.action(Motor.STOP)
                    time.sleep(0.1)
                    motor.action(Motor.BACK)
                    time.sleep(0.3)
                    motor.action(Motor.STOP)
                   
            
